import os
import sys
import threading
import time
import serial

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")))
from tools import string_to_chunks
from log_info import logger


class Sensor:
    def __init__(self, get_ser=None, direct="right"):
        self.det_stop = False
        self.det_pause = False
        self.res = [0, 2]
        self.ser = None
        self.direct = direct
        self.index = {"left": "05", "right": "06"}
        if get_ser is not None:
            self.get_server(get_ser)

    def init(self):
        if self.ser is not None:
            det_process = threading.Thread(target=self.det)
            det_process.setDaemon(True)
            det_process.start()
        else:
            logger.info("sensor还没有获取服务")

    def get_server(self, car):
        self.ser = car.base_moto.ser

    def close(self):
        self.det_stop = True
        logger.info("sensor close")

    def det(self):
        while True:
            if self.det_stop:
                break
            if self.det_pause:
                continue
            command = f"77 68 09 07 03 {self.index[self.direct]} 00 00 0a"
            command_bytes = bytes.fromhex(command)
            ret, res = self.ser.get_answer(command_bytes)
            if ret:
                res = string_to_chunks(res.hex(), 2)
                if len(res) == 9:
                    self.res = [int(res[-3], 16), int(res[-2], 16)]


class sensor_test(Sensor):
    def __init__(self):
        super(Sensor, self).__init__()
        self.ser = serial.Serial('/dev/ttyUSB0', 1000000)
        self.det_stop = False
        self.det_pause = False
        self.res = [0, 2]
        det_process = threading.Thread(target=self.det)
        det_process.setDaemon(True)
        det_process.start()

    def det(self):
        command = f"77 68 09 07 03 05 00 00 0a"
        command_bytes = bytes.fromhex(command)
        res = ''
        n = 0
        while True:
            if self.det_stop:
                break
            if self.det_pause:
                continue
            self.ser.write(command_bytes)
            while True:
                res += " " + self.ser.read().hex()
                n += 1
                if n == 9:
                    self.res = [int(res.split(" ")[-3], 16), int(res.split(" ")[-2], 16)]
                    sys.stdout.write(f'\r{self.res[0]},{self.res[1]}')
                    sys.stdout.flush()
                    res = ''
                    n = 0
                    break


if __name__ == '__main__':
    sen = sensor_test()
    while True:
        time.sleep(0.1)
